#include "LidarDriverX.h"

#pragma region Init
LidarDriverX::LidarDriverX(QWidget *parent, RSDriverParam param)
    : QWidget(parent), rs_driver_param(param)
{
    param.print();

    // // 创建VTK渲染窗口
    // vtkSmartPointer<vtkRenderWindow> renderWindow = vtkSmartPointer<vtkGenericOpenGLRenderWindow>::New();
    // this->setRenderWindow(renderWindow);
    // auto vtk_interactor = vtkSmartPointer<vtkRenderWindowInteractor>::New();
    // vtk_interactor->SetRenderWindow(renderWindow);

    // 初始化PCL可视化器
    // pcl_viewer = std::make_shared<PCLVisualizer>(vtk_interactor, "RSPointCloudViewer", false);
    pcl_viewer = std::make_shared<PCLVisualizer>("RSPointCloudViewer");
    pcl_viewer->setBackgroundColor(0.0, 0.0, 0.0);
    pcl_viewer->addCoordinateSystem(1.0);
    pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_pointcloud(new pcl::PointCloud<pcl::PointXYZI>);
    pcl_viewer->addPointCloud<pcl::PointXYZI>(pcl_pointcloud, "rslidar");
    pcl_viewer->setPointCloudRenderingProperties(PCL_VISUALIZER_POINT_SIZE, 2, "rslidar");

    driver.regExceptionCallback(std::bind(&LidarDriverX::exceptionCallback, this, std::placeholders::_1));
    driver.regPointCloudCallback(std::bind(&LidarDriverX::pointCloudGetCallback, this), std::bind(&LidarDriverX::pointCloudPutCallback, this, std::placeholders::_1));

    // renderWindow->AddRenderer(pcl_viewer->getRenderWindow()->GetRenderers()->GetFirstRenderer());
    // this->setEnableHiDPI(true);
    // this->interactor()->SetRenderWindow(renderWindow);
    // this->interactor()->Initialize();
}

LidarDriverX::~LidarDriverX()
{
}

void LidarDriverX::Start()
{
    to_exit_process = false;

    if (!driver.init(rs_driver_param))
    {
        RS_ERROR << "Driver Initialize Error..." << RS_REND;
        return;
    }

    RS_INFO << "Lidar-Driver Viewer start..." << RS_REND;
    _threadCloudHandle.setName("CloudHanlde");
    _threadCloudHandle.start(&LidarDriverX::threadCloudHandle, this);
    driver.start();

    timer = new QTimer(this);
    connect(timer, &QTimer::timeout, this, &LidarDriverX::updatePointCloud);
    timer->start(100);
}

void LidarDriverX::Stop()
{
    driver.stop();
    to_exit_process = true;
    cloud_handle_thread.join();
}
#pragma endregion

#pragma region Process
void LidarDriverX::threadCloudHandle(void *pContext)
{
    LidarDriverX *pHandler = static_cast<LidarDriverX *>(pContext);
    pHandler->processCloud();
}

void LidarDriverX::processCloud()
{
    // qDebug() << "process cloud test.";
    while (!to_exit_process)
    {
        std::shared_ptr<PointCloudMsg> msg = stuffed_cloud_queue.popWait();
        if (msg.get() == NULL)
        {
            continue;
        }

        // RS_MSG << "msg: " << msg->seq << " point cloud size: " << msg->points.size() << RS_REND;

        pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_pointcloud(new pcl::PointCloud<pcl::PointXYZI>);
        pcl_pointcloud->points.swap(msg->points);
        pcl_pointcloud->height = msg->height;
        pcl_pointcloud->width = msg->width;
        pcl_pointcloud->is_dense = msg->is_dense;

        PointCloudColorHandlerGenericField<pcl::PointXYZI> point_color_handle(pcl_pointcloud, "intensity");

        {
            const std::lock_guard<std::mutex> lock(mtx_viewer);
            pcl_viewer->removePointCloud("rslidar");
            pcl_viewer->addPointCloud<pcl::PointXYZI>(pcl_pointcloud, point_color_handle, "rslidar");
        }

        free_cloud_queue.push(msg);
    }
}
#pragma endregion

#pragma region Callback
void LidarDriverX::exceptionCallback(const Error &code)
{
    RS_WARNING << code.toString() << RS_REND;
}

std::shared_ptr<PointCloudMsg> LidarDriverX::pointCloudGetCallback(void)
{
    // qDebug() << "pointCloudGetCallback";
    std::shared_ptr<PointCloudMsg> msg = free_cloud_queue.pop();
    if (msg.get() != NULL)
    {
        return msg;
    }

    return std::make_shared<PointCloudMsg>();
}

void LidarDriverX::pointCloudPutCallback(std::shared_ptr<PointCloudMsg> msg)
{
    // qDebug() << "pointCloudPutCallback";
    stuffed_cloud_queue.push(msg);
}
#pragma endregion

#pragma region SLOT
void LidarDriverX::updatePointCloud()
{
    {
        const std::lock_guard<std::mutex> lock(mtx_viewer);
        pcl_viewer->spinOnce();
    }
}
#pragma endregion